package openglsuperbible.glutils;
import org.lwjgl.BufferUtils;
import org.lwjgl.opengl.GL11;
import org.lwjgl.util.vector.Matrix3f;
import org.lwjgl.util.vector.Matrix4f;
import org.lwjgl.util.vector.Quaternion;
import org.lwjgl.util.vector.Vector3f;

import java.nio.FloatBuffer;

import static java.lang.Math.cos;
import static java.lang.Math.sin;
import static java.lang.Math.sqrt;

/**
 *
 * @author Matthew
 */
public class MatrixUtils {
    private static FloatBuffer scratchFloatMatrix = BufferUtils.createFloatBuffer(16);

    private static void loadScratch(Matrix4f m) {
        scratchFloatMatrix.rewind();
        m.store(scratchFloatMatrix);
        scratchFloatMatrix.rewind();
    }

    private static void loadScratch(Matrix3f m) {
        scratchFloatMatrix.rewind();
        m.store(scratchFloatMatrix);
        scratchFloatMatrix.put(new float[]{0, 0, 0});
        scratchFloatMatrix.rewind();
    }

    public static void glMultMatrix(Matrix4f m) {
        loadScratch(m);
        GL11.glMultMatrix(scratchFloatMatrix);
    }

    public static void glMultMatrix(Matrix3f m) {
        loadScratch(m);
        GL11.glMultMatrix(scratchFloatMatrix);
    }

    public static void glLoadMatrix(Matrix4f m) {
        loadScratch(m);
        GL11.glLoadMatrix(scratchFloatMatrix);
    }

    public static void glLoadMatrix(Matrix3f m) {
        loadScratch(m);
        GL11.glLoadMatrix(scratchFloatMatrix);
    }

    
    public static Matrix3f makeRotationMatrix(float angle, float x, float y, float z,
                                              boolean normalize, Matrix3f m) {
        if (m == null)
            m = new Matrix3f();

        if (x == 0 && y == 0 && z == 0) {
            
            m.setIdentity();
            return m;
        }

        if (normalize) {
            double length = sqrt(x * x + y * y + z * z);
            // normalize
            x /= length;
            y /= length;
            z /= length;
        }

        double s = sin(angle);
        double c = cos(angle);

        
        double cc = 1.0f - c;

        m.m00 = (float) ((cc * x * x) + c);
        m.m01 = (float) ((cc * x * y) - z * s);
        m.m02 = (float) ((cc * z * x) + y * s);

        m.m10 = (float) ((cc * x * y) + z * s);
        m.m11 = (float) ((cc * y * y) + c);
        m.m12 = (float) ((cc * y * z) - x * s);

        m.m20 = (float) ((cc * z * x) - y * s);
        m.m21 = (float) ((cc * y * z) + x * s);
        m.m22 = (float) ((cc * z * z) + c);

        return m;
    }


    public static Matrix3f makeRotationMatrix(float angle, float x, float y, float z) {
        return makeRotationMatrix(angle, x, y, z, true);
    }

    public static Matrix3f makeRotationMatrix(float angle, float x, float y, float z, boolean normalize) {
        Matrix3f ignored = null; // portable way to shut up @Nullable warning
        return makeRotationMatrix(angle, x, y, z, normalize, ignored);
    }


    public static Matrix3f makeRotationmatrix(float angle, Vector3f v) {
        Matrix3f ignored = null; // portable way to shut up @Nullable warning
        return makeRotationMatrix(angle, v.getX(), v.getY(), v.getZ(), false, ignored);
    }

    public static Matrix3f makeRotationmatrix(float angle, Vector3f v, Matrix3f m) {
        return makeRotationMatrix(angle, v.getX(), v.getY(), v.getZ(), false, m);
    }

    public static Matrix3f makeRotationMatrix(Quaternion q, Matrix3f m) {
        if (m == null)
            m = new Matrix3f();

        float qx = q.getX();
        float qy = q.getY();
        float qz = q.getZ();
        float qw = q.getW();


        float qx2 = qx*qx;
        float qy2 = qy*qy;
        float qz2 = qz*qz;
    
        m.m00 = 1 - 2 * qy2 - 2 * qz2;
        m.m01 = 2 * qx * qy + 2 * qz * qw;
        m.m02 = 2 * qx * qz - 2 * qy * qw;

        m.m10 = 2 * qx * qy - 2 * qz * qw;
        m.m11 = 1 - 2 * qx2 - 2 * qz2;
        m.m12 = 2 * qy * qz + 2 * qx * qw;

        m.m20 = 2 * qx * qz + 2 * qy * qw;
        m.m21 = 2 * qy * qz - 2 * qx * qw;
        m.m22 = 1 - 2 * qx2 - 2 * qy2;

        return m;
    }

    public static Matrix3f makeRotationMatrix(Quaternion q) {
        Matrix3f ignored = null;
        return makeRotationMatrix(q, ignored);
    }
}
